#include <filter_algorithm.h>

//-------------------------- 限幅滤波算法 -------------------------//
//方法：
//根据经验判断两次采样允许的最大偏差值（设为A）
//每次检测到新值时判断：
//a. 如果本次值与上次值之差<=A，则本次值有效
//b. 如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值
//优点：
//能有效克服因偶然因素引起的脉冲干扰
//缺点:
//无法抑制周期性的干扰
//平滑度差
rt_uint32_t FA_Amplitude_Limiting_Filtering_Algorithm(rt_uint8_t A, rt_uint32_t old_value, rt_uint32_t new_value)
{
    rt_uint32_t final_value;

    if (fabs(new_value - old_value) > A)
    {
        final_value = old_value;
    }
    else
    {
        final_value = new_value;
    }
    return final_value;
}

//-------------------------- 中位值滤波算法 -------------------------//
//方法:
//连续采样N次（N取奇数），把N次采样值按大小排列
//取中间值为本次有效值
//优点:
//能有效克服因偶然因素引起的脉冲干扰
//对温度、液位等缓慢变化的物理量有良好的滤波效果
//缺点:
//对流量、速度等快速变化的物理量不适用
rt_uint32_t FA_Median_Value_Filtering_Algorithm(rt_uint8_t N, rt_uint32_t *value_buf)
{
    rt_uint8_t i, j;
    rt_uint32_t temp = 0, final_value = 0;

    for (j = 0; j < N - 1; j++)
    {
        for (i = 0; i < N - j - 1; i++)
        {
            if (value_buf[i] > value_buf[i + 1])
            {
                temp = value_buf[i];
                value_buf[i] = value_buf[i + 1];
                value_buf[i + 1] = temp;
            }
        }
    }
    final_value = value_buf[(N - 1) / 2];
    return final_value;
}

//-------------------------- 算术平均滤波算法 --------------------------//
//方法：
//连续取N个采样值进行算术平均运算
//N值较大时：信号平滑度较高，但灵敏度较低
//N值较小时：信号平滑度较低，但灵敏度较高
//N值的选取：一般流量，N=12；压力：N=4
//优点:
//适用于一般具有随机干扰的信号进行滤波
//信号在某一数值范围附近上下波动
//缺点:
//对测量速度较慢的物理量不适用
//对需要计算速度快的实时控制不适用
//比较浪费RAM
rt_uint32_t FA_Arithmetic_Average_Filtering_Algorithm(rt_uint8_t N, rt_uint32_t *value_buf)
{
    rt_uint8_t count;
    rt_uint32_t temp = 0, final_value = 0;

    for (count = 0; count < N; count++)
    {
        temp += value_buf[count];
    }
    final_value = temp / N;
    return final_value;
}

//-------------------------- 递推平均滤波算法--------------------------//
//方法:
//把连续取N个采样值看成一个队列
//队列的长度固定为N
//每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据.(先进先出原则)
//把队列中的N个数据进行算术平均运算，就可获得新的滤波结果
//N值的选取：流量，N=12；压力：N=4；液面，N=4 ~ 12；温度，N=1 ~ 4
//优点:
//对周期性干扰有良好的抑制作用
//平滑度高
//适用于高频振荡系统
//缺点:
//灵敏度低
//对偶然出现的脉冲性干扰的抑制作用较差
//不易消除由脉冲干扰引起的采样值偏差
//不适用于脉冲干扰比较严重的场合
//比较浪费RAM
rt_uint32_t FA_Recursive_Average_Filtering_Algorithm(rt_uint8_t N, rt_uint32_t *value_buf)
{
    rt_uint8_t i;
    rt_uint32_t temp, final_value;
    for (i = 0; i < N; i++)
    {
        value_buf[i] = value_buf[i + 1];
        temp += value_buf[i];
    }
    final_value = temp / N;
    return final_value;
}

//-------------------------- 中位值平均滤波算法 --------------------------//
//方法:
//相当于“中位值滤波法”+“算术平均滤波法”
//连续采样N个数据，去掉一个最大值和一个最小值
//然后计算N-2个数据的算术平均值
//N值的选取：3~14
//优点：
//融合两种滤波算法的优点
//对于偶然出现的脉冲性干扰，可消除由于脉冲干扰引起的采样值偏差
//缺点：
//测量速度较慢
//比较浪费RAM
rt_uint32_t FA_Median_Value_Average_Filtering_Algorithm(rt_uint8_t N, rt_uint32_t *value_buf)
{
    rt_uint8_t i, j, count;
    rt_uint32_t temp = 0, sum = 0, final_value = 0;

    for (j = 0; j < N - 1; j++)
    {
        for (i = 0; i < N - j - 1; i++)
        {
            if (value_buf[i] > value_buf[i + 1])
            {
                temp = value_buf[i];
                value_buf[i] = value_buf[i + 1];
                value_buf[i + 1] = temp;
            }
        }
    }
    for (count = 1; count < N - 1; count++)
    {
        sum += value_buf[count];
    }
    final_value = sum / (N - 2);

    return final_value;
}

//-------------------------- 限幅平均滤波算法 --------------------------//
//方法:
//相当于“限幅滤波法”+“递推平均滤波法”
//每次采样到的新数据先进行限幅处理，
//再送入队列进行递推平均滤波处理
//优点：
//融合两种滤波算法的优点
//对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差
//缺点：
//比较浪费RAM
rt_uint32_t FA_Limiting_Average_Filtering_Algorithm(rt_uint8_t A,
                                                 rt_uint8_t N,
                                                 rt_uint32_t old_value,
                                                 rt_uint32_t new_value)
{
    rt_uint8_t i = 0, count;
    rt_uint32_t sum, final_value;
    rt_uint32_t value_buf[N];

    if (fabs(new_value - old_value) < A)
    {
        value_buf[i++] = new_value;
    }
    if (i == N)
    {
        i = 0;
    }
    for (count = 0; count < N; count++)
    {
        sum += value_buf[count];
    }
    final_value = sum / N;

    return final_value;
}

//-------------------------- 一阶滞后滤波算法 --------------------------//
//方法:
//取a=0~100
//本次滤波结果=（1-a）本次采样值+a上次滤波结果
//优点：
//对周期性干扰具有良好的抑制作用
//适用于波动频率较高的场合
//缺点：
//相位滞后，灵敏度低
//滞后程度取决于a值大小
//不能消除滤波频率高于采样频率的1/2的干扰信号
rt_uint32_t FA_First_Order_Lag_Filtering_Algorithm(rt_uint8_t a, rt_uint32_t old_value, rt_uint32_t new_value)
{
    rt_uint32_t final_value;

    final_value = (100 - a) * old_value + a * new_value;
    return final_value;
}

//-------------------------- 加权递推平均滤波算法 --------------------------//
//方法:
//是对递推平均滤波法的改进，即不同时刻的数据加以不同的权
//通常是，越接近现时刻的数据，权取得越大。
//给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低
//优点：
//适用于有较大纯滞后时间常数的对象和采样周期较短的系统
//缺点：
//对于纯滞后时间常数较小，采样周期较长，变化缓慢的信号
//不能迅速反应交易系统当前所受干扰的严重程度，滤波效果差
rt_uint32_t FA_Weighted_Recursive_Average_Filtering_Algorithm(rt_uint8_t N, rt_uint32_t *value_buf)
{
    rt_uint8_t count;
    rt_uint32_t sum, final_value;
    rt_uint8_t coe[N];
    rt_uint8_t sum_coe = 0;

    for (int i = 0; i < N; i++)
    {
        coe[i] = i + 1;
        sum_coe += coe[i];
    }
    for (count = 0; count < N; count++)
    {
        sum += value_buf[count] * coe[count];
    }
    final_value = sum / sum_coe;

    return final_value;
}

//-------------------------- 消抖滤波算法 --------------------------//
//方法:
//设置一个滤波计数器
//将每次采样值与当前有效值比较
//如果采样值=当前有效值，则计数器清零
//如果采样值>或<当前有效值，则计数器+1，并判断计数器是否>=上线N(溢出)
//如果计数器溢出，则将本次值替换当前有效值，并清计数器
//优点：
//对于变化缓慢的被测参数有较好的滤波效果
//可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
//缺点：
//对于快速变化的参数不宜
//如果在计数器溢出的那一次采样到的值恰好是干扰值，则会将干扰值当作有效值导入系统。
rt_uint32_t FA_Vibration_Elimination_Filtering_Algorithm(rt_uint8_t N,
                                                      rt_uint32_t old_value,
                                                      rt_uint32_t new_value)
{
    rt_uint8_t count = 0;
    rt_uint32_t final_value;

    if (new_value == old_value)
    {
        count = 0;
    }
    else
    {
        count++;
        if (count > N)
        {
            count = 0;
            final_value = new_value;
        }
    }
    return final_value;
}

//-------------------------- 限幅消抖滤波算法 --------------------------//
//方法:
//相当于“限幅滤波法”+“消抖滤波法”
//先限幅，后消抖
//优点：
//继承了“限幅”和“消抖”的优点
//改进了“消抖滤波法”中的某些缺陷
//避免将干扰值导入系统。
//缺点：
//对于快速变化的参数不宜
rt_uint32_t FA_Limiting_Vibration_Elimination_Filtering_Algorithm(rt_uint8_t A,
                                                               rt_uint8_t N,
                                                               rt_uint32_t old_value,
                                                               rt_uint32_t new_value)
{
    rt_uint8_t count = 0;
    rt_uint32_t temp, final_value;

    if (fabs(new_value - old_value) > A)
    {
        temp = old_value;
    }
    else
    {
        temp = new_value;
    }
    if (new_value == old_value)
    {
        count = 0;
    }
    else
    {
        count++;
        if (count > N)
        {
            count = 0;
            final_value = temp;
        }
    }
    return final_value;
}

//-------------------------- 卡尔曼滤波算法 --------------------------//
//方法:
//根据当前测量值和上一刻的预测量和误差，计算当前最优量，再预测下一刻的量
//将误差纳入计算，分为预测误差和测量误差
//误差独立存在，始终不受测量数据影响
//优点：
//递归算法，不需要存储所有历史数据，节省存储空间和计算时间
//满足一定条件下，可以达到最小均方误差的极限(卡尔曼边界)
//不同场景通用算法
//缺点：
//基于模型算法，模型不准确或分布不符合假设，性能会降低。
//只适用于线性系统或近似线性系统。
//非线性或非高斯系统，需要使用拓展卡尔曼滤波器或无迹卡尔曼滤波器等变体或扩展，增加计算复杂度和误差来源
//依赖初始条件，需要给定初始状态估计和初始误差协方差矩阵，初始条件不合理或不准确会导致收敛时间较长或无法收敛
rt_uint32_t FA_Kalman_Filtering_Algorithm(rt_uint32_t old_value, rt_uint32_t new_value)
{
    float prevDate = old_value;
    float p = 10;
    float q = 0.001;
    float r = 0.001;
    float kGain = 0;
    rt_uint32_t final_value;

    p = p + q;
    kGain = p / (p + r);
    new_value = prevDate + (kGain * (new_value - prevDate));
    p = (1 - kGain) * p;
    prevDate = new_value;
    final_value = new_value;

    return final_value;
}

